
#include "testcase.hpp"

int main()
{
	long nFramSeq = 0;

	cv::Mat img0;
	cv::Mat img1;
	cv::Mat img2;
	cv::Mat img3;

	//open video file
	cv::VideoCapture capture("G:/OnTheFly_TestData/test_dataset/test1/20170606110939.ts");
	string strFileName = "20170606110939.ts";
	std::stringstream ss;
	std::string loadImagePath;
	if( !capture.isOpened() )
	{
		std::cout<<"fail to open video file!"<<std::endl;
		return -1;
	}

	long totalFrameNumber = (long)capture.get(CV_CAP_PROP_FRAME_COUNT);
	long frameToStart = 0;
	capture.set( CV_CAP_PROP_POS_FRAMES,frameToStart);

	cv::Mat srcImg_big;
	nFramSeq = frameToStart;

	while(nFramSeq < totalFrameNumber)
	{
		capture >> srcImg_big;
		if( !srcImg_big.data )
		{
			cout << "read frame image fail!" << endl;
			break;
		}
		cout << "nFrameSeq:" << nFramSeq << endl;
		//cv::Mat srcImg_big = cv::imread("./test_dataset/test1/201767929329.jpg",cv::IMREAD_COLOR);
		//img0 = cv::imread("./src1.jpg",cv::IMREAD_COLOR);
		//img1 = cv::imread("./src2.jpg",IMREAD_COLOR);
		img0 = srcImg_big( cv::Rect(0,240,256,480) ); //Front
		img1 = srcImg_big( cv::Rect(256,240,256,480) ); //Right
		img2 = srcImg_big( cv::Rect(512,240,256,480) ); //Back
		img3 = srcImg_big( cv::Rect(768,240,256,480) ); //Left

		//SURFMatchTest(img0(Rect(127,0,128,240)),img1(Rect(127,0,128,240)));
		//SIFTMatchTest(img1(Rect(127,0,128,240)),img0(Rect(127,0,128,240)));
		//ORBMatchTest(img1(Rect(127,0,128,240)),img0(Rect(127,0,128,240)));
		//FASTMatchTest(img1(Rect(127,0,128,240)),img0(Rect(127,0,128,240)));
		HarrisMatchTest(img1(Rect(127,0,128,240)),img0(Rect(127,0,128,240)));

		nFramSeq++;
	}

	capture.release();

	return 0;
}

int SURFMatchTest( Mat &image01, Mat &image02 )
{
	//Mat image01 = imread("g2.jpg", 1);    
	//Mat image02 = imread("g4.jpg", 1);    
	imshow("p2", image01);
	imshow("p1", image02);

	//灰度图转换  
	Mat image1, image2;
	cvtColor(image01, image1, CV_RGB2GRAY);
	cvtColor(image02, image2, CV_RGB2GRAY);


	//提取特征点    
	SurfFeatureDetector surfDetector(2000);  // 海塞矩阵阈值，在这里调整精度，值越大点越少，越精准 
	vector<KeyPoint> keyPoint1, keyPoint2;
	surfDetector.detect(image1, keyPoint1);
	surfDetector.detect(image2, keyPoint2);

	//特征点描述，为下边的特征点匹配做准备    
	SurfDescriptorExtractor SurfDescriptor;
	Mat imageDesc1, imageDesc2;
	SurfDescriptor.compute(image1, keyPoint1, imageDesc1);
	SurfDescriptor.compute(image2, keyPoint2, imageDesc2);

	FlannBasedMatcher matcher;
	vector<vector<DMatch> > matchePoints;
	vector<DMatch> GoodMatchePoints;

	vector<Mat> train_desc(1, imageDesc1);
	matcher.add(train_desc);
	matcher.train();

	matcher.knnMatch(imageDesc2, matchePoints, 2);
	cout << "total match points: " << matchePoints.size() << endl;

	// Lowe's algorithm,获取优秀匹配点
	for (int i = 0; i < matchePoints.size(); i++)
	{
		if (matchePoints[i][0].distance < 0.6 * matchePoints[i][1].distance)
		{
			GoodMatchePoints.push_back(matchePoints[i][0]);
		}
	}

	for( auto i : GoodMatchePoints )
	{
		cout << i.distance << endl;
	}

	Mat first_match;
	drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
	imshow("first_match ", first_match);
	waitKey();
	keyPoint1.clear();
	keyPoint2.clear();
	GoodMatchePoints.clear();
	train_desc.clear();
	matchePoints.clear();

	destroyAllWindows();

	return 0;
}

int SIFTMatchTest( Mat &image01, Mat &image02 )
{
	imshow("p2", image01);
	imshow("p1", image02);

	//灰度图转换  
	Mat image1, image2;
	cvtColor(image01, image1, CV_RGB2GRAY);
	cvtColor(image02, image2, CV_RGB2GRAY);


	//提取特征点    
	SiftFeatureDetector siftDetector(800);  // 海塞矩阵阈值，在这里调整精度，值越大点越少，越精准 
	vector<KeyPoint> keyPoint1, keyPoint2;
	siftDetector.detect(image1, keyPoint1);
	siftDetector.detect(image2, keyPoint2);

	//特征点描述，为下边的特征点匹配做准备    
	SiftDescriptorExtractor SiftDescriptor;
	Mat imageDesc1, imageDesc2;
	SiftDescriptor.compute(image1, keyPoint1, imageDesc1);
	SiftDescriptor.compute(image2, keyPoint2, imageDesc2);

#if 1
	FlannBasedMatcher matcher;
	vector<vector<DMatch> > matchePoints;
	vector<DMatch> GoodMatchePoints;

	vector<Mat> train_desc(1, imageDesc1);
	matcher.add(train_desc);
	matcher.train();

	matcher.knnMatch(imageDesc2, matchePoints, 2);
	cout << "total match points: " << matchePoints.size() << endl;

	// Lowe's algorithm,获取优秀匹配点
	for (int i = 0; i < matchePoints.size(); i++)
	{
		if (matchePoints[i][0].distance < 0.6 * matchePoints[i][1].distance)
		{
			GoodMatchePoints.push_back(matchePoints[i][0]);
		}
	}
	Mat first_match;
	drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
#else
	//实例化暴力匹配器——BruteForceMatcher  
	BruteForceMatcher<L2<float>> matcher;    
	//定义匹配器算子  
	vector<DMatch>matches;    
	//实现描述符之间的匹配，得到算子matches  
	matcher.match(imageDesc1,imageDesc2,matches);
	cout << "matches size : " << matches.size() << endl;
	//提取出前30个最佳匹配结果  
	//std::nth_element(matches.begin(),     //匹配器算子的初始位置  
	//	matches.begin()+5,     // 排序的数量  
	//	matches.end());       // 结束位置  
	////剔除掉其余的匹配结果  
	//matches.erase(matches.begin()+5, matches.end());
	Mat first_match;
	drawMatches(image02, keyPoint2, image01, keyPoint1, matches, first_match);
#endif
	
	imshow("first_match ", first_match);
	//imwrite("first_match.jpg", first_match);
	waitKey();
	destroyAllWindows();

	return 0;
}

int ORBMatchTest( Mat &image01, Mat &image02 )
{
	imshow("p2", image01);
	imshow("p1", image02);

	//灰度图转换  
	Mat image1, image2;
	cvtColor(image01, image1, CV_RGB2GRAY);
	cvtColor(image02, image2, CV_RGB2GRAY);


	//提取特征点    
	OrbFeatureDetector OrbDetector(1000);  // 在这里调整精度，值越小点越少，越精准 
	vector<KeyPoint> keyPoint1, keyPoint2;
	OrbDetector.detect(image1, keyPoint1);
	OrbDetector.detect(image2, keyPoint2);

	//特征点描述，为下边的特征点匹配做准备    
	OrbDescriptorExtractor OrbDescriptor;
	Mat imageDesc1, imageDesc2;
	OrbDescriptor.compute(image1, keyPoint1, imageDesc1);
	OrbDescriptor.compute(image2, keyPoint2, imageDesc2);

	flann::Index flannIndex(imageDesc1, flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);

	vector<DMatch> GoodMatchePoints;

	Mat macthIndex(imageDesc2.rows, 2, CV_32SC1), matchDistance(imageDesc2.rows, 2, CV_32FC1);
	flannIndex.knnSearch(imageDesc2, macthIndex, matchDistance, 2, flann::SearchParams());

	// Lowe's algorithm,获取优秀匹配点
	for (int i = 0; i < matchDistance.rows; i++)
	{
		if (matchDistance.at<float>(i,0) < 0.6 * matchDistance.at<float>(i, 1))
		{
			DMatch dmatches(i, macthIndex.at<int>(i, 0), matchDistance.at<float>(i, 0));
			GoodMatchePoints.push_back(dmatches);
		}
	}

	Mat first_match;
	drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
	imshow("first_match ", first_match);
	//imwrite("first_match.jpg", first_match);
	waitKey();
	destroyAllWindows();
	return 0;
}

int FASTMatchTest( Mat &image01, Mat &image02 )
{
	imshow("p2", image01);
	imshow("p1", image02);

	//灰度图转换  
	Mat image1, image2;
	cvtColor(image01, image1, CV_RGB2GRAY);
	cvtColor(image02, image2, CV_RGB2GRAY);


	//提取特征点    
	FastFeatureDetector Detector(50);  //阈值 
	vector<KeyPoint> keyPoint1, keyPoint2;
	Detector.detect(image1, keyPoint1);
	Detector.detect(image2, keyPoint2);

	//特征点描述，为下边的特征点匹配做准备    
	SiftDescriptorExtractor   Descriptor;
	Mat imageDesc1, imageDesc2;
	Descriptor.compute(image1, keyPoint1, imageDesc1);
	Descriptor.compute(image2, keyPoint2, imageDesc2);

	BruteForceMatcher< L2<float> > matcher;   
	vector<vector<DMatch> > matchePoints;
	vector<DMatch> GoodMatchePoints;

	vector<Mat> train_desc(1, imageDesc1);
	matcher.add(train_desc);
	matcher.train();

	matcher.knnMatch(imageDesc2, matchePoints, 2);
	cout << "total match points: " << matchePoints.size() << endl;

	// Lowe's algorithm,获取优秀匹配点
	for (int i = 0; i < matchePoints.size(); i++)
	{
		if (matchePoints[i][0].distance < 0.6 * matchePoints[i][1].distance)
		{
			GoodMatchePoints.push_back(matchePoints[i][0]);
		}
	}

	Mat first_match;
	drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
	imshow("first_match ", first_match);
	//imwrite("first_match.jpg", first_match);
	waitKey();
	destroyAllWindows();
	return 0;
}

int HarrisMatchTest( Mat &image01, Mat &image02 )
{
	imshow("p2", image01);
	imshow("p1", image02);

	//灰度图转换  
	Mat image1, image2;
	cvtColor(image01, image1, CV_RGB2GRAY);
	cvtColor(image02, image2, CV_RGB2GRAY);


	//提取特征点    
	GoodFeaturesToTrackDetector Detector(500);  //最大点数,值越大，点越多
	vector<KeyPoint> keyPoint1, keyPoint2;
	Detector.detect(image1, keyPoint1);
	Detector.detect(image2, keyPoint2);

	//特征点描述，为下边的特征点匹配做准备    
	SiftDescriptorExtractor  Descriptor;
	Mat imageDesc1, imageDesc2;
	Descriptor.compute(image1, keyPoint1, imageDesc1);
	Descriptor.compute(image2, keyPoint2, imageDesc2);

	BruteForceMatcher< L2<float> > matcher;   
	vector<vector<DMatch> > matchePoints;
	vector<DMatch> GoodMatchePoints;

	vector<Mat> train_desc(1, imageDesc1);
	matcher.add(train_desc);
	matcher.train();

	matcher.knnMatch(imageDesc2, matchePoints, 2);
	cout << "total match points: " << matchePoints.size() << endl;

	// Lowe's algorithm,获取优秀匹配点
	for (int i = 0; i < matchePoints.size(); i++)
	{
		if (matchePoints[i][0].distance < 0.6 * matchePoints[i][1].distance)
		{
			GoodMatchePoints.push_back(matchePoints[i][0]);
		}
	}

	Mat first_match;
	drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
	imshow("first_match ", first_match);
	//imwrite("first_match.jpg", first_match);
	waitKey();
	destroyAllWindows();

	return 0;
}